#include "interface_header.hpp"
#include "JointGroupCtrlClient.hpp"
#include "owndebug.h"
#include <curses.h>

int main(int argc, char *argv[]){

debugSetGlobalDebugLvl(2);
MarsNamespace::MarsManipulator test("localhost",40002,"mars","Control");
bool run= true;
	test.resetMicro();

	

	initscr();
	  cbreak();
	  //noecho();
	  nonl();
	  intrflush(stdscr, FALSE);
	  keypad(stdscr, TRUE);
	  scrollok(stdscr, TRUE);
	  //nodelay(stdscr, TRUE);
	  //halfdelay(10);

	  // After this point, we don't want dPrint to print to Screen, so we disable it.
	  debugSetPrintLevel(2);

	  unsigned int speedSeq = 0;
	  unsigned int positionSeq = 0;
	  //unsigned int forceSeq = 0;
	  clear();
	  printw("Welcome to IO test program.\n\n");
	  using namespace MaCI::JointGroupCtrl;
	do {
	    int value;
	    printw("\nSelect JointGroupCtrl Query:\n");
	    printw(

		   "q = QUIT\n");
	    printw("?\n");

	    value = getch();
	    clear(); // After input, clear the screen. Not after processing!
	    //MaCI::JointGroupCtrl::EJointGroupStatus currentstatus;
	    //jgcc.GetJointGroupStatus(currentstatus,1000);
	    ownSleep_ms(100);
	  switch(value){

	 //get current position and add an increment to it to move step by step
	    	case KEY_UP:{//x
		test.moveStepX();
		break;
		}
		case KEY_DOWN:{//-x
		test.movenegStepX();
		break;
	    	}
	    	case KEY_LEFT:{//-y
		test.moveStepY();
		break;
	    	}
	    	case KEY_RIGHT:{//y
		test.movenegStepY();
		break;
	    	}
            case '0':{//y
	    	printw("Enter shoulder angle: ");
	    	int angle;
            scanw("%d",&angle);
	    	test.moveShoulder(angle);
		break;
	    	}
            case '1':{//y
	    	printw("Enter elbow angle: ");
	    	int angle;
            scanw("%d",&angle);
	    	test.moveElbow(angle);
		break;
	    	}
	       	case '4':{//y
	    	printw("Enter wrist angle: ");
	    	int angle;
            scanw("%d",&angle);
	    	test.moveWrist(angle);
		break;
	    	}
            case '7':{//y
	    	printw("Enter gripper angle: ");
	    	int angle;
            scanw("%d",&angle);
	    	test.moveGripper(angle);
		break;
	    	}
            case '8':{//y
	    	test.toTop();
		break;
	    	}
            case '2':{//y
            test.toBottom();
		break;
	    	}
            case '5':{//y
            test.grabObject();
		break;
	    	}
            case 's':{//y
            printw("store to which pocket: ");
	    	int angle;
            scanw("%d",&angle);
	    	test.storeObjectToPocket(angle);
		break;
	    	}
            case 'r':{//y
            test.retractArm(0);
		break;
	    	}
            case 'e':{//y
            test.extendArm();;
		break;
	    	}
	    	case 'm':{//y
            printw("move to x _ y: ");
	    	int angle, ypos;
            scanw("%d %d",&angle, &ypos);
	    	test.pickObject(angle,ypos);
		break;
	    	}
	    	case 'c':{//y
            printw("move with c=0 to x _ y: ");
	    	int angle, ypos;
            scanw("%d %d",&angle, &ypos);
	    	test.movewithconstCto(angle,ypos);
		break;
	    	}
	    	case 'q':{
	    	    run= false;
	    	    break;
	    	}

		default:{
	      	clear();
	      	printw("\nUnknown command.\n\n");
	      	continue;
	     	break;
	    	}
	    }
	} while (run);
}
